package com.zhdl.network.service.impl;

import com.project.command.faq.event.behavior.BehaviorPerform;
import com.project.command.instruction.bo.SingleInstruction;
import com.project.common.util.ByteHelper;
import com.zhdl.common.util.IdUtil;
import com.zhdl.modules.tools.IntToHexUtil;
import com.zhdl.modules.tools.PublicTools;
import com.zhdl.network.demotcp.analysis.MemoBusAnalysisImpl;
import com.zhdl.network.modbus.request.WriteSingleKeepRegisterReq;
import com.zhdl.network.modbustcp.ModbusTcpRespPkg;
import com.zhdl.network.modbustcp.after.ModbusAfterService;
import com.zhdl.network.modbustcp.analysis.ModbusAnalysis;
import com.zhdl.network.service.RobotArmService;
import com.zhdl.network.service.conf.PortConf;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import javax.annotation.Resource;
import java.io.IOException;

import static com.project.command.instruction.strategy.abs.AbsDic.INSTRUCTION_DEFAULT_HEX;

@Slf4j
@Service
public class RobotArmServiceImpl implements RobotArmService {
//    public static int[] UVW0 = new int[]{-178990, 9707, -178794};
    /* 0为竖向，1为横向 */
//    public static int[] UVW0 = new int[]{-179700, 0, -90000};
//    public static int[] UVW1 = new int[]{-179700, 0, -179700};
public static int[] UVW = new int[]{180000, 0, 180000};
//    public static int originZ = 1525802;
    public static int originZ = 1100000;

//    public static int originZInit = 2125802;
//    public static int orientation = -1;


    @Resource
    private BehaviorPerform behaviorPerform;

    @Resource
    private PortConf portConf;

    /**
     * 通过字节数组转换成 16进制的String类型
     *
     * @param location 世界坐标
     * @return 16进制的世界坐标
     */
    public static String getHexByIntArray(int[] location) {
        StringBuilder stringBuffer = new StringBuilder();
        for (int j : location) {
            String s = IntToHexUtil.intToHex(j);
            stringBuffer.append(s);
        }
        return stringBuffer.toString();
    }

    /**
     * 发送指令 - 参考模板
     */
    @Override
    public void sendCmd() {
        SingleInstruction instruction = new SingleInstruction();
        instruction.setInstructionId(IdUtil.getUUID_8());
        instruction.setInstruction("cmd");
        instruction.setBeforeInstructionName(INSTRUCTION_DEFAULT_HEX);
        //默认使用com.zhdl.instruction.strategy包下后置策略
        instruction.setAfterInstructionName(ModbusAfterService.MODBUS_TCP_AFTER_SERVICE);
        //客户端通信地址, 串口为数字, 网口为ip+端口       数据来源 : port_comm 或 port_socket 表
        instruction.setPortKey("8");
        //设置协议  数据来源 : protocols 表
        instruction.setProtocols(ModbusAnalysis.MODBUS_TCP_ANALYSIS);
        behaviorPerform.perform(instruction);
    }

    /**
     * 发送指令 - 灵活
     *
     * @param cmd 指令
     */
    @Override
    public void sendCmd(String cmd) {
        SingleInstruction instruction = new SingleInstruction();
        instruction.setInstructionId(IdUtil.getUUID_8());
        instruction.setInstruction(cmd);
        instruction.setBeforeInstructionName(INSTRUCTION_DEFAULT_HEX);
        //默认使用com.zhdl.instruction.strategy包下后置策略
        instruction.setAfterInstructionName(ModbusAfterService.MODBUS_TCP_AFTER_SERVICE);
        //客户端通信地址, 串口为数字, 网口为ip+端口       数据来源 : port_comm 或 port_socket 表
//        instruction.setPortKey("127.0.0.1:5020");
        instruction.setPortKey(portConf.getTcpPortRobot());
        //设置协议  数据来源 : protocols 表
//        instruction.setProtocols(ModbusAnalysis.MODBUS_TCP_ANALYSIS);
        instruction.setProtocols(MemoBusAnalysisImpl.MODBUS_TCP_ANALYSIS);
        behaviorPerform.perform(instruction);
    }


    /**
     * 机器人-机械臂 初始化
     */
    public void robotWriteArmInit() {
        String status = PublicTools.getString("cmd", "robotWriteArmInit");
        sendCmd(status);
        log.info("机器人-机械臂 初始化");
    }

    /**
     * 读取当前状态 --暂且改为 读取世界坐标
     * - ready 待机 run 运动
     */
    @Override
    public void getRobotStatus() {
        String status = PublicTools.getString("cmd", "robotReadArmStatus");
        sendCmd(status);
        log.info("读取机器人当前状态");
    }

    /**
     * 机器人-写06 暂停 - 启动
     */
    @Override
    public void robotWritePause(boolean open){
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setId("01");
        String address = open ? "4E21" : "4E23";
        writeManyKeepRegisterReq.setStartAddr(address);
        writeManyKeepRegisterReq.setWriteValue("0001");
        try {
            byte[] bytes = ModbusTcpRespPkg.WriteSingleKeepRegister(writeManyKeepRegisterReq);
            sendCmd(ByteHelper.bytesArrayToHexString(bytes));
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 读取 M10- 160(A0)至 M114线圈值
     *
     * @param address 地址
     */
    @Override
    public void readWitchStatus(int address) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setId("01");
        writeManyKeepRegisterReq.setStartAddr(IntToHexUtil.shortToHex(address));
        writeManyKeepRegisterReq.setWriteValue("0001");
        try {
            byte[] bytes = ModbusTcpRespPkg.read01(writeManyKeepRegisterReq);
            sendCmd(ByteHelper.bytesArrayToHexString(bytes));
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 设置 M10- 160(A0)至 M114线圈值
     *
     * @param address 地址
     * @param open    发送的通知
     */
    @Override
    public void writeWitchStatus(int address, boolean open) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setId("01");
        writeManyKeepRegisterReq.setStartAddr(IntToHexUtil.shortToHex(address));
        writeManyKeepRegisterReq.setWriteValue(open ? "FF00" : "0000");

        try {
            byte[] bytes = ModbusTcpRespPkg.write05(writeManyKeepRegisterReq);
            log.info(ByteHelper.bytesArrayToHexString(bytes));
            sendCmd(ByteHelper.bytesArrayToHexString(bytes));
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 设置 机器人抓取点 800(558C)-805
     *
     * @param photoLocation 照片返回坐标
     */
    @Override
    public void pickLocation(int[] photoLocation) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setStartAddr("558C");
        writeManyKeepRegisterReq.setId("01");
//        if (orientation == 0) {
//            log.info("竖向姿态");
//            writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(photoLocation) + getHexByIntArray(UVW0));
//        } else {
//            log.info("横向姿态");
//            writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(photoLocation) + getHexByIntArray(UVW1));
//        }

        writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(photoLocation) + getHexByIntArray(UVW));

        try {
            byte[] bytes = ModbusTcpRespPkg.WriteManyKeepRegister(writeManyKeepRegisterReq);
            String s = ByteHelper.bytesArrayToHexString(bytes);
            sendCmd(s);
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 设置机器人 上电上抬位置 820(55A0)-825
     *
     * @param currentLocation 设置初始话的坐标Z轴
     */
    @Override
    public void initUpLocation(int[] currentLocation) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setStartAddr("55B4");
        writeManyKeepRegisterReq.setId("01");
        currentLocation[2] = originZ;
        writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(currentLocation));
        try {
            byte[] bytes = ModbusTcpRespPkg.WriteManyKeepRegister(writeManyKeepRegisterReq);
            String s = ByteHelper.bytesArrayToHexString(bytes);
            sendCmd(s);
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

    /**
     * 设置 机器人抓取点上方 810(5596)-815
     *
     * @param photoLocation 照片返回坐标 - Z轴坐标
     */
    @Override
    public void pickUpLocation(int[] photoLocation) {
        WriteSingleKeepRegisterReq writeManyKeepRegisterReq = new WriteSingleKeepRegisterReq();
        writeManyKeepRegisterReq.setStartAddr("55A0");
        writeManyKeepRegisterReq.setId("01");
        photoLocation[2] = originZ;
//        if (orientation == 0) {
//            writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(photoLocation) + getHexByIntArray(UVW0));
//        } else {
//            writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(photoLocation) + getHexByIntArray(UVW1));
//        }
        writeManyKeepRegisterReq.setWriteValue(getHexByIntArray(photoLocation) + getHexByIntArray(UVW));
        try {
            byte[] bytes = ModbusTcpRespPkg.WriteManyKeepRegister(writeManyKeepRegisterReq);
            String s = ByteHelper.bytesArrayToHexString(bytes);
            sendCmd(s);
        } catch (IOException e) {
            throw new RuntimeException(e);
        }
    }

}
